#include "Kalman.h"



// 初始化卡尔曼滤波器
void kalmanFilterInit(KalmanFilter *kf, float Q, float R, float initialX, float initialP) {
    kf->Q = Q;
    kf->R = R;
    kf->X = initialX;
    kf->P = initialP;
}

// 更新卡尔曼滤波器
float kalmanFilterUpdate(KalmanFilter *kf, float measurement) {
    // 预测步骤
    kf->X = kf->X;
    kf->P = kf->P + kf->Q;

    // 更新步骤
    kf->K = kf->P / (kf->P + kf->R);
    kf->X = kf->X + kf->K * (measurement - kf->X);
    kf->P = (1 - kf->K) * kf->P;

    return kf->X;
}
